#ifndef _GRID_H_
#define _GRID_H_

#include <iostream>
#include <vector>
#include <list>
#include <map>
#include <math.h>

using namespace std;

// Basic structure for representing a coordinate in world space.
struct Coord {
   double x,y;
   
   Coord(double _x,double _y) : x(_x),y(_y){}
};

// Basic structure for representing a coordinate in discrete space.
struct DCoord {
   int x,y;
   
   DCoord(int _x,int _y) : x(_x),y(_y){}
   bool operator==(const DCoord &comp){
      return comp.x == this->x && comp.y == this->y;
   }
};


// Structure used to represent an individual cell in a graph. It maintains
// the location of the cell in both workspace coordinates and c-space coordinates.
// It also contains representation of the likelihood that the exact position
// is an obstacle (wVal), and the likelihood that the c-space cell this cell
// belongs to contains an object (cVal).
struct GridCoord {
   public:
      enum SearchState {NEW,OPEN,CLOSED};
      
      // Grid variables
      DCoord wPos; // World-Space position
      DCoord cPos; // C-Space position
      double wVal;
      double cVal;
      
      // A-Star variables
      GridCoord *from;

      GridCoord(DCoord _pos,double _val) : wPos(_pos),cPos(0,0),wVal(_val),cVal(_val){
         from = 0;
      }
      
};

ostream& operator<<(ostream& out, const GridCoord& x);
ostream& operator<<(ostream& out, const DCoord& x);

// Represents a grid.
class Grid {
   private:
      // How many pixels in the workspace represent one pixel in cspace.
      int cSpace_pixels;
      
      double meters_per_pixel;

   public:
      // 0 = top left of grid
      vector<vector<GridCoord> > grid;
      
      // Function that takes in a gridfile object in binary format (each byte
      // represents a single pixel in the grid, with 00 signifying no
      // obstacle and FF signifying an obstacle.
      static vector<vector<GridCoord> > parse_gridFile(char *filename,int width,int height);
      
      // Converts the coordinate passed in from workspace coordinates to
      // c-space coordinates.
      DCoord workCoords_to_cCoords(DCoord c);
      
      // Converts the coordinate passed in from c-space coordinates to
      // workspace coordinates.
      DCoord cCoords_to_workCoords(DCoord coords);

      // Returns the coordinates of the provided grid cell in world-space.
      Coord get_worldCoord(GridCoord *at);
     
      // Constructs a grid using the passed-in grid as its representation.
      Grid(vector<vector<GridCoord> > grid);
      
      // Constructs a grid using the passed-in grid a its representation. mpp
      // represents the number of meters per pixel, and bs represents the size
      // of the box around the robot.
      Grid(vector<vector<GridCoord> > grid,double mpp,double bs);
      
      // Returns the grid coordinate located at the world-space coordinate
      // passed in.
      GridCoord *get_gridCoord(const Coord);
      
      // Returns the grid coordinate located at the work-space coordinate
      // passed in.
      GridCoord *get_gridCoord(const DCoord);
      
      // Returns the grid coordinate located at the c-space coordinate
      // passed in.
      GridCoord *get_gridCoordFromCSpace(const DCoord);
      
      // Allows a user to get a grid coordinate by using array syntax.
      GridCoord *operator[](const DCoord);
      GridCoord *operator[](const Coord);
      
      // Allows a user to get a grid coordinate by passing in the x and y
      // coordinates in the workspace.
      GridCoord *operator()(int,int);
      
      // Returns the distance between two grid coordinates, based on their position
      // in c-space>
      double dist_between(const GridCoord *from,const GridCoord *to);
      
      // Returns the (up to) eight neighbors of the specified coordinate
      list<GridCoord*> get_neighbors(const GridCoord *from);
      
      // Reduces the number of vertices in a path by removing all vertices
      // that fall between two vertices in a straight line.
      static list<GridCoord*> vertex_reduce(list<GridCoord*> path);
};

#endif
